#include "commonRobotInfoDice.h"

//Declare variables and constants
const float JOYSTICK_SCALE = 0.78;
int joystick1_x = 0;
int joystick1_y = 0;
int joystick2_x = 0;
int joystick2_y = 0;
int currHeading = 0;
bool setStraight = false;
bool straightMode = false;
float slowMode = 1;
bool hookButtonPressed = false;
bool launcherButtonPressed = false;
bool isSweeperRunning = false;
bool redirectButtonPressed = false;
bool isRedirectUp = true;
bool armButtonPressed = false;




//Declare functions
bool areJoysticksCentered (int joy1_x, int joy1_y, int joy2_x);

void initializeRobot();

void redirectUp();

task runLauncher();

task toggleRedirect();

task isStraight();

void movement(int scaledX , int scaledY, int scaledTheta, float isSlow);

void initializeRobot()
{
	bSystemLeaveServosEnabledOnProgramStop = false;
	HTGYROstartCal(HTGYRO);
	bFloatDuringInactiveMotorPWM = false;
	halt();
	servo[BallRedirect] = REDIRECT_UP;//Put all servos in proper positions.
	servo[Hook] = SERVO_LEFT_UP;
	servo[KickArm] = KICK_IN;
	servo[CenterGoal] = CENTER_GOAL_UP;
	StartTask(updateSensors);//Start timed tasks for updating of certain mechanisms.
	StartTask(updateGyro);
	StartTask(runLauncher);
	StartTask(toggleRedirect);
	StartTask(isStraight);
	motor[Lift] = 0;
	HTEOPDsetLongRange(HTEOPD);
}

//Returns true if joysticks are centered, false if not.
bool areJoysticksCentered(int joy1_x,int joy1_y, int joy2_x)
{
	return ((joy1_x > -7 && joy1_x < 7) && (joy1_y > -7 && joy1_y < 7) && (joy2_x > -7 && joy2_x < 7));
}

void redirectUp()//Toggles the ball redirect.
{
	servoChangeRate[BallRedirect] = 10;
	//if (!isRedirectUp)//If redirect is down (or in any position other than up)
	//{
	//	//servo[BallRedirect] = REDIRECT_MID;
	//	//while (servo[BallRedirect] != REDIRECT_MID)
	//	//{
	//	//}
	//	//servo[BallRedirect] = REDIRECT_UP;
	//	//isRedirectUp = true;
	//}
	//else
	//{
		servo[BallRedirect] = REDIRECT_MID;
		wait10Msec(50);
		//while (ServoValue[BallRedirect] != REDIRECT_MID)
		//{
		//}
		servo[BallRedirect] = REDIRECT_DOWN;
		isRedirectUp = false;
	//}
}




task runLauncher()//Controls toggling of launcher motor
{
	float motorPower = 0;
	while(true)
	{
		if (joy1Btn(3) == 1)
		{
			if (!launcherButtonPressed)
			{
				if (motor[Sweeper] == 0)//Ramps up motor
				{
					isSweeperRunning = true;
					while (motor[Sweeper] < 100)
					{
						motor[Sweeper] = motorPower;
						motorPower += 0.033;
					}
				}
				else//Ramps down motor
				{
					bFloatDuringInactiveMotorPWM = true;
					motor[Sweeper] = 0;
					wait10Msec(75);
					bFloatDuringInactiveMotorPWM = false;
					motorPower = 0;
					isSweeperRunning = false;
				}
				launcherButtonPressed = true;
			}
		}
		else
		{
			launcherButtonPressed = false;
		}
		EndTimeSlice();
	}
}

task toggleRedirect()//Toggles the ball redirect servo.
{
	while (true)
	{
		if (joy1Btn(2) == 1)
		{
			if (!redirectButtonPressed)
			{
				redirectUp();
				redirectButtonPressed = true;
			}
		}
		else
		{
			redirectButtonPressed = false;
		}
		wait1Msec(1);
	}
}

void movement(int scaledX ,int scaledY,int scaledTheta, float isSlow)//Moves the robot based on joystick values.
{
	int newTheta = scaledTheta;
	//if (straightMode)
	//{
	//	newTheta +=(currHeading - heading);
	//	writeDebugStreamLine("Difference: %d", newTheta);
	//}
	motor[Front] = (scaledY + -scaledX - newTheta) * isSlow;//Incorporates omnidirectional movement.
	motor[Right] = ((-scaledY) + (-scaledX) - newTheta) * isSlow;
	motor[Left] = ((scaledY) + (scaledX) - newTheta) * isSlow;
	motor[Rear] = ((-scaledY) + (scaledX) - newTheta) * isSlow;
}

task isStraight()
{
	int currVal;
	int prevVal;
	currVal = joystick.joy1_x2;
	while (true)
	{
		prevVal = currVal;
		currVal = joystick.joy1_x2;
		if ((currVal < 7 && currVal > -7))
		{
			//writeDebugStreamLine("It's straight");
			straightMode = true;
			if (currVal != prevVal)
			{
				setStraight = true;
				currHeading = heading;
				//writeDebugStreamLine("Setting straight: %d", currHeading);
			}
			else
			{
				setStraight = false;
			}
			//writeDebugStreamLine("Difference from heading: %d", currHeading - heading);
		}
		else
		{
			//writeDebugStreamLine("It's not straight");
			straightMode = false;
		}
		wait10Msec(5);
	}
}
